#include "usrRemote.h"
#include <stdio.h>
#include <unistd.h>
#include <math.h>

const double DEG2RAD1 = 3.1415926 / 180.0;

int main()
{
    simxInt errId = simx_return_ok;
    errId = InitVrepClient();
    printf("errId: %d\n", errId);
    if (errId != simx_return_ok)
    {
        return 0;
    }
    double jPosCmd[6] = {0};
    double jPosfb[6] = {0};
    jPosCmd[0] = 0;
    double t = 0;
    while (t < 1000 * 1)
    {
        jPosCmd[0] = 100.0 * sin(0.008 * t);
        jPosCmd[1] = 50.0 * sin(0.008 * t);
        jPosCmd[2] = -60.0 * sin(0.008 * t);
        swapJointPosData(jPosCmd, jPosfb);
        t = t + 8.0;
        // printf("t:%f, jPosCmd: %f, jPosfb: %f\n", t, jPosCmd[0]*DEG2RAD1, jPosfb[0]/DEG2RAD1);
        sleep(0.016);
    }

    closeVrepSimulator();
    return 0;
}
